For a more detailed information on the goals, and specific of the project, please refer to the README file.
import cv2
import glob
import numpy as np
import os
import pickle
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
%matplotlib inline
### group calibration and undistortion together
def cal_undistort(img, objpoints, imgpoints):
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, img.shape[0:2], None, None)
undist = cv2.undistort(img, mtx, dist, None, mtx)
return undist, mtx, dist
### test undistorting images with camera calibration
test_image_fname = './camera_cal/calibration3.jpg'
test_image = mpimg.imread(test_image_fname)
test_gray = cv2.cvtColor(test_image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(test_gray, (6, 9), None)
if ret == True:
cv2.drawChessboardCorners(test_image, (6, 9), corners, ret)
plt.imshow(test_image)
plt.title("Distorted Chessboard (file3) image with corner points")
plt.show()
### import the calibration images and calibrate camera to undistort
images = glob.glob('./camera_cal/calibration*.jpg')
## interpolating 2d space to 3d space
objpoints = []
imgpoints = []
# x,y coordinate combinations
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9, 0:6].T.reshape(-1,2)
# for each file, convert to grayscale, find corners
for fname in images:
img = mpimg.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9,6), None)
if ret == True:
print(fname)
imgpoints.append(corners)
objpoints.append(objp)
### Output undistorted test image and the mts and distortion matrix
test_undist, mtx, dist = cal_undistort(test_image, objpoints, imgpoints)
### OPTIONAL: Save the outputs to a pickle file.
output_file = "distortion.p"
if not os.path.isfile(output_file):
print('Pickle file not found. Saving data to pickle file.')
dist_data_pickle = {}
dist_data_pickle["mtx"] = mtx
dist_data_pickle["dist"] = dist
pickle.dump(dist_data_pickle, open(output_file,"wb"))
else:
print('Pickle file is already found. Skip this step')
### Compare the test images (undistorted and distorted) side by side
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(test_image)
ax1.set_title('Distorted Image')
ax2.imshow(test_undist)
ax2.set_title('Undistorted Image')
'''
Sobel gradient thresholding
INPUT
orient: orientation of the sobel gradient - x or y
thresh: threshold for the gradient (low, high)
'''
def abs_threshold(img, orient='x', sobel_kernel=3, thresh=(0,255)):
# 1) Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# 2) Sobel gradient depending on the orientation
if orient == 'x':
sobel = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
elif orient == 'y':
sobel = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
return sobel
abs_sobel = np.absolute(sobel)
scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
sobel_binary = np.zeros_like(scaled_sobel)
sobel_binary[(scaled_sobel >= thresh[0])\
& (scaled_sobel <= thresh[1])] = 1
return sobel_binary
# Helper function for the rest of the gradient thresholds
def sobel_operator_ksize(img, sobel_kernel=3):
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Take both Sobel x and y gradients
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
return sobelx, sobely
'''
Gradient magnitude thresholding
'''
def mag_threshold(img, sobel_kernel=3, mag_thresh=(0, 255)):
sobelx, sobely = sobel_operator_ksize(img, sobel_kernel)
# Calculate the gradient magnitude
gradmag = np.sqrt(sobelx**2 + sobely**2)
# Rescale to 8 bit
scale_factor = np.max(gradmag)/255
gradmag = (gradmag/scale_factor).astype(np.uint8)
# Create a binary image of ones where threshold is met, zeros otherwise
binary_output = np.zeros_like(gradmag)
binary_output[(gradmag >= mag_thresh[0])\
& (gradmag <= mag_thresh[1])] = 1
# Return the binary image
return binary_output
'''
Gradient direction thresholding
'''
def dir_threshold(img, sobel_kernel=3, dir_thresh=(0, np.pi/2)):
sobelx, sobely = sobel_operator_ksize(img, sobel_kernel)
# Take the absolute value of the gradient direction,
# apply a threshold, and create a binary image result
absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))
binary_output = np.zeros_like(absgraddir)
binary_output[(absgraddir >= dir_thresh[0])\
& (absgraddir <= dir_thresh[1])] = 1
# Return the binary image
return binary_output
'''
HLS color thresholding
'''
def hls_threshold(img, thresh=(0,255)):
# 1) Convert to HLS color space
hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
# 2) Apply a threshold to the S channel
binary_output = np.zeros_like(hls[:,:,2])
# 3) Return a binary image of threshold result
binary_output[(hls[:,:,2] <= thresh[1])\
& (hls[:,:,2] >= thresh[0])] = 1
return binary_output
def masking_region(img, vertices):
mask = np.zeros_like(img)
#defining a 3 channel or 1 channel color to fill the mask with depending on the input image
if len(img.shape) > 2:
channel_count = img.shape[2]
ignore_mask_color = (255,) * channel_count
else:
ignore_mask_color = 255
#filling pixels inside the polygon defined by "vertices" with the fill color
cv2.fillPoly(mask, vertices, ignore_mask_color)
#returning the image only where mask pixels are nonzero
masked_image = cv2.bitwise_and(img, mask)
return masked_image
Thresholding methods are combined to best detect lanes. From there, we will be masking to best obtain lane lines.
def combined_thresh(img):
ksize = 7
threshx = (50, 255)
threshy = (10, 255)
mag_thresh = (50, 255)
dir_thresh = (0.7, 1.3)
hls_thresh = (170, 255)
gradx = abs_threshold(img, orient='x', sobel_kernel=ksize, thresh=threshx)
grady = abs_threshold(img, orient='y', sobel_kernel=ksize, thresh=threshy)
mag_binary = mag_threshold(img, sobel_kernel=ksize, mag_thresh=mag_thresh)
dir_binary = dir_threshold(img, sobel_kernel=ksize, dir_thresh=dir_thresh)
hls_binary = hls_threshold(img, hls_thresh)
combined_binary = np.zeros_like(dir_binary)
combined_binary[((gradx == 1)) | ((mag_binary == 1) & (dir_binary == 1)) | (hls_binary == 1)] = 1
imshape = img.shape
left_bottom = (100, imshape[0])
right_bottom = (imshape[1]-20, imshape[0])
apex1 = (610, 410)
apex2 = (680, 410)
inner_left_bottom = (310, imshape[0])
inner_right_bottom = (1150, imshape[0])
inner_apex1 = (700,480)
inner_apex2 = (650,480)
vertices = np.array([[left_bottom, apex1, apex2, \
right_bottom, inner_right_bottom, \
inner_apex1, inner_apex2, inner_left_bottom]], dtype=np.int32)
# Masked area
combined_binary = masking_region(combined_binary, vertices)
return combined_binary
result_test_images = glob.glob('./test_images/test*.jpg')
for fname in result_test_images:
pre_thresh = mpimg.imread(fname)
post_thresh = combined_thresh(pre_thresh)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(pre_thresh)
ax1.set_title('Original Image', fontsize=40)
ax2.imshow(post_thresh, cmap='gray')
ax2.set_title('Threshold Result', fontsize=40)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
image_shape = pre_thresh.shape
print(image_shape)
# Define the region
area_of_interest = [[150+430,460],[1150-440,460],[1150,720],[150,720]]
def perspective_transform(img, mtx, dist):
undist = cv2.undistort(img, mtx, dist, None, mtx)
offset_x = 200
offset_y = 0
img_size = (pre_thresh.shape[1], pre_thresh.shape[0])
src = np.float32(area_of_interest)
dst = np.float32([[offset_x, offset_y],
[img_size[0]-offset_x, offset_y],
[img_size[0]-offset_x, img_size[1]-offset_y],
[offset_x, img_size[1]-offset_y]])
M = cv2.getPerspectiveTransform(src, dst)
Minv = cv2.getPerspectiveTransform(dst, src)
warped = cv2.warpPerspective(undist, M, img_size)
return warped, M, Minv
# Visualizing the perspective transform
top_down, perspective_M, inverse_M = perspective_transform(pre_thresh, mtx, dist)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(pre_thresh)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(top_down)
ax2.set_title('Undistorted and Warped Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
top_down_gray, perspective_M, inverse_M = perspective_transform(post_thresh, mtx, dist)
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(post_thresh, cmap='gray')
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(top_down_gray, cmap='gray')
ax2.set_title('Undistorted and Warped Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
# As suggested, we define a class to receive the characteristics of each line detection
# change to fp format later...
class Line():
def __init__(self):
# was the line detected in the last iteration?
self.detected = False
# x values of the last n fits of the line
self.recent_xfitted = []
#average x values of the fitted line over the last n iterations
self.bestx = None
#polynomial coefficients averaged over the last n iterations
self.best_fit = None
#polynomial coefficients for the most recent fit
self.current_fit = [np.array([False])]
#radius of curvature of the line in some units
self.radius_of_curvature = None
#distance in meters of vehicle center from the line
self.line_base_pos = None
#difference in fit coefficients between last and new fits
self.diffs = np.array([0,0,0], dtype='float')
#x values for detected line pixels
self.allx = None
#y values for detected line pixels
self.ally = None
# Looking at the line class, some of the methods that need to be establish include..
def find_peaks(image):
histogram = np.sum(image[image.shape[0]/2:,:], axis=0)
midpoint = np.int(histogram.shape[0]/2)
leftx_base = np.argmax(histogram[:midpoint])
rightx_base = np.argmax(histogram[midpoint:]) + midpoint
return leftx_base, rightx_base
def calculate_average_coefficients(existing_coefficient, new_coefficient):
coefficient = existing_coefficient
return coefficient
def find_radius_of_curvature(yvals, fitx, left_or_right='r'):
# Define y-value where we want radius of curvature
# I'll choose the maximum y-value, corresponding to the bottom of the image
y_eval = np.max(yvals)
ym_per_pix = 30/720 # meters per pixel in y dimension
xm_per_pix = 3.7/700 # meters per pixel in x dimension
if left_or_right == 'l' or 'L':
left_curverad = ((1 + (2*fitx[0]*y_eval*ym_per_pix + fitx[1])**2)**1.5) / np.absolute(2*fitx[0])
return left_curverad
elif left_or_right == 'r' or 'R':
right_curverad = ((1 + (2*fitx[0]*y_eval*ym_per_pix + fitx[1])**2)**1.5) / np.absolute(2*fitx[0])
return right+curverad
else:
print("Invalid direction was input.")
def find_position(pts):
# Find the position of the car from the center
# It will show if the car is 'x' meters from the left or right
position = image_shape[1]/2
left = np.min(pts[(pts[:,1] < position) & (pts[:,0] > 700)][:,1])
right = np.max(pts[(pts[:,1] > position) & (pts[:,0] > 700)][:,1])
center = (left + right)/2
# Define conversions in x and y from pixels space to meters
xm_per_pix = 3.7/700 # meteres per pixel in x dimension
return (position - center)*xm_per_pix
def predict_using_polynomials(left_lane, right_lane, image_shape):
yvals = np.linspace(0, 100, num=101)*7.2
left_lane_fitx = left_lane.current_fit[0]*yvals**2 + left_lane.current_fit[1]*yvals + left_lane.current_fit[2]
right_lane_fitx = right_lane.current_fit[0]*yvals**2 + right_lane.current_fit[1]*yvals + right_lane.current_fit[2]
return yvals, left_lane_fitx, right_lane_fitx
# Set the width of the windows +/- margin
#margin = 100
# Set minimum number of pixels found to recenter window
#minpix = 50
# Choose the number of sliding windows
#nwindows = 9
# Currently this isn't fail proof. If a lane disappears and then reappears
# there is no way of readjusting..
def fit_lane(left_lane, right_lane, image, nwindows, margin, minpix):
if left_lane.detected == True & right_lane.detected == True:
left_lane_inds = ((left_lane.allx > (left_lane.current_fit[0]*(left_lane.ally**2) +\
left_lane.current_fit[1]*left_lane.ally +\
left_lane.current_fit[2] - margin)) &\
(left_lane.allx < (left_lane.current_fit[0]*(left_lane.ally**2) +\
left_lane.current_fit[1]*left_lane.ally +\
left_lane.current_fit[2] + margin)))
right_lane_inds = ((right_lane.allx > (right_lane.current_fit[0]*(right_lane.ally**2) +\
right_lane.current_fit[1]*right_lane.ally +\
right_lane.current_fit[2] - margin)) &\
(right_lane.allx < (right_lane.current_fit[0]*(right_lane.ally**2) +\
right_lane.current_fit[1]*right_lane.ally +\
right_lane.current_fit[2] + margin)))
left_lane.allx = nonzerox[left_lane_inds]
left_lane.ally = nonzeroy[left_lane_inds]
right_lane.allx = nonzerox[right_lane_inds]
right_lane.ally = nonzeroy[right_lane_inds]
left_fit = np.polyfit(left_lane.ally, left_lane.allx, 2)
right_fit = np.polyfit(right_lane.ally, right_lane.allx, 2)
# push polynomial to lanes
left_lane.current_fit = left_fit
right_lane.current_fit = right_fit
# Calculate x with polynomials calculated.
yvals, left_fitx, right_fitx = predict_using_polynomials(left_lane, right_lane, image.shape)
yvals = np.linspace(0, 100, num=101)*7.2
# measure curvature
left_lane.radius_of_curvature = find_radius_of_curvature(yvals, left_fitx, 'l')
right_lane.radius_of_curvature = find_radius_of_curvature(yvals, right_fitx, 'r')
return yvals, left_fitx, right_fitx
else:
# get coords for the binary values we are interested in
nonzero = image.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
window_height = np.int(image.shape[0]/nwindows)
left_lane_inds = []
right_lane_inds = []
# find initial peaks to draw the left and right lanes
leftx_base, rightx_base = find_peaks(image)
leftx_current = leftx_base
rightx_current = rightx_base
# dividing the image up into n windows
for window in range(nwindows):
# Identify window boundaries in x and y (and right and left)
win_y_low = image.shape[0] - (window+1)*window_height
win_y_high = image.shape[0] - window*window_height
win_xleft_low = leftx_current - margin
win_xleft_high = leftx_current + margin
win_xright_low = rightx_current - margin
win_xright_high = rightx_current + margin
# Identify the nonzero pixels in x and y within the window
good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high)\
& (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0]
good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high)\
& (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0]
# Append these indices to the lists
left_lane_inds.append(good_left_inds)
right_lane_inds.append(good_right_inds)
# If you found > minpix pixels, recenter next window on their mean position
if len(good_left_inds) > minpix:
leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
if len(good_right_inds) > minpix:
rightx_current = np.int(np.mean(nonzerox[good_right_inds]))
# Concatenate the arrays of indices
left_lane_inds = np.concatenate(left_lane_inds)
right_lane_inds = np.concatenate(right_lane_inds)
# Extract left and right line pixel positions
left_lane.allx = nonzerox[left_lane_inds]
left_lane.ally = nonzeroy[left_lane_inds]
right_lane.allx = nonzerox[right_lane_inds]
right_lane.ally = nonzeroy[right_lane_inds]
# Fit a second order polynomial to each
left_fit = np.polyfit(left_lane.ally, left_lane.allx, 2)
right_fit = np.polyfit(right_lane.ally, right_lane.allx, 2)
# push polynomial to lanes
left_lane.current_fit = left_fit
right_lane.current_fit = right_fit
# Calculate x with polynomials calculated.
yvals, left_fitx, right_fitx = predict_using_polynomials(left_lane, right_lane, image.shape)
yvals = np.linspace(0, 100, num=101)*7.2
# measure curvature
left_lane.radius_of_curvature = find_radius_of_curvature(yvals, left_fitx, 'l')
right_lane.radius_of_curvature = find_radius_of_curvature(yvals, right_fitx, 'r')
left_lane.detected == True
right_lane.detected == True
return yvals, left_fitx, right_fitx
left_lane = Line()
right_lane = Line()
yvals, left_fitx, right_fitx = fit_lane(left_lane, right_lane, top_down_gray, 9, 100, 100)
left_fit = left_lane.current_fit
right_fit = right_lane.current_fit
plt.imshow(top_down_gray, cmap='gray')
plt.plot(left_fitx, yvals, color='yellow')
plt.plot(right_fitx, yvals, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
print("left lane curvature is", left_lane.radius_of_curvature)
print("right lane curvature is", right_lane.radius_of_curvature)
# main process should consist of masking, perspective transform, and polyfit. Also should have layover on image
def draw_lane_on_image(original_image, warped_image, yvals, left_fitx, right_fitx, Minv):
# Create an image to draw the lines on
warp_zero = np.zeros_like(warped_image).astype(np.uint8)
color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
# Recast the x and y points into usable format for cv2.fillPoly()
pts_left = np.array([np.transpose(np.vstack([left_fitx, yvals]))])
pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, yvals])))])
pts = np.hstack((pts_left, pts_right))
# Draw the lane onto the warped blank image
cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))
# Warp the blank back to original image space using inverse perspective matrix (Minv)
newwarp = cv2.warpPerspective(color_warp, Minv, (original_image.shape[1], original_image.shape[0]))
# Combine the result with the original image
result = cv2.addWeighted(original_image, 1, newwarp, 0.3, 0)
font_scale=1
thickness=2
font = cv2.FONT_HERSHEY_SIMPLEX
text = "Radius of Curvature: {} m".format(int(left_lane.radius_of_curvature))
cv2.putText(result,text,(400,100), font, font_scale,(255,255,255), thickness)
# Find the position of the car
pts = np.argwhere(newwarp[:,:,1])
position = find_position(pts)
if position < 0:
text = "Vehicle is {:.2f} m left of center".format(-position)
else:
text = "Vehicle is {:.2f} m right of center".format(position)
cv2.putText(result,text,(400,200), font, 1,(255,255,255),2)
return result
def advanced_lane_detection(image):
# Mask and filter the colors to mark out regions of interest
img = combined_thresh(image)
# Warp the image to make lanes parallel to each other
top_down, perspective_M, perspective_Minv = perspective_transform(img, mtx, dist)
# Find the lines fitting to left and right lanes
yvals, l_fitx, r_fitx = fit_lane(left_lane, right_lane, top_down, 9, 100, 100)
# Return the original image with colored region
return draw_lane_on_image(image, top_down, yvals, l_fitx, r_fitx, perspective_Minv)
### Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML
# Set up lines for left and right
left_lane = Line()
right_lane = Line()
white_output = 'output.mp4'
clip1 = VideoFileClip("project_video.mp4")
white_clip = clip1.fl_image(advanced_lane_detection) #NOTE: this function expects color images!!
%time white_clip.write_videofile(white_output, audio=False)